package robot;

import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;

import lejos.nxt.Button;
import lejos.nxt.Motor;
import lejos.nxt.comm.Bluetooth;
import lejos.nxt.comm.NXTConnection;

public class Robot implements Runnable {

	static String devName = "linvor";
	static String devAddress = "00:12:12:21:27:42";
	private NXTConnection connection;
	private static DataInputStream in;
	private static DataOutputStream out;
	private boolean running = false;

	private int motor_speed;

	public Robot() {
		System.out.println("Waiting for BT\nconnection...");
		Bluetooth.btEnable();
		Bluetooth.setName("NXT");
		Bluetooth.setPower(true);
		Bluetooth.setVisibility((byte) 1);
		try {
			// btc = Bluetooth.waitForConnection(20000,
			// NXTConnection.PACKET);
			 connection = Bluetooth.waitForConnection(20000,
						 NXTConnection.RAW);
			//connection = Bluetooth.connect(devName, NXTConnection.RAW);

		} catch (Exception e) {
			System.out.println("No connection...\n");
			Button.waitForAnyPress();
			return;
		}
		System.out.println("Connection OK!\n");
		in = connection.openDataInputStream();
		out = connection.openDataOutputStream();
	}

	public void run() {

		running = true;
		System.out.println("Running Robot...");
		try{
		while (true){
			
			String cmd = get_command();
			//System.out.println("NEW CMD: " + cmd);
			
			if (cmd.equalsIgnoreCase("FW")){ // move forward
				Motor.A.forward();
				Motor.B.forward();
			}else if(cmd.equalsIgnoreCase("BW")){
				Motor.A.backward();
				Motor.B.backward();
			}else if (cmd.equalsIgnoreCase("HS")){ // high speed
				Motor.A.setSpeed(200);
				Motor.B.setSpeed(200);
			}else if (cmd.equalsIgnoreCase("LS")){ // low speed
				Motor.A.setSpeed(50);
				Motor.B.setSpeed(50);
			}else if (cmd.equalsIgnoreCase("RF")){ // rotate right forward
				Motor.B.stop();
				Motor.A.forward();
			}else if (cmd.equalsIgnoreCase("LF")){ // rotate left forward
				Motor.A.stop();
				Motor.B.forward();
			}else if (cmd.equalsIgnoreCase("RB")){ // rotate right backward
				Motor.B.stop();
				Motor.A.backward();
			}else if (cmd.equalsIgnoreCase("LB")){ // rotate left backward
				Motor.A.stop();
				Motor.B.backward();
			}else if (cmd.equalsIgnoreCase("ST")){ // stop
				Motor.A.stop();
				Motor.B.stop();
			}
			
		}
		}catch(IOException e){
			System.out.println("IO Error!");
			running = false;
		}
	}

	private String get_command() throws IOException{
		
		if (in == null){
			return "NOPE";
		}
		String ret = "";
		
		byte b = in.readByte();
		while (b != 13 ){ // ASCII \r
		
			ret += (char) b;
			b = in.readByte();
		}
		
		
		return ret;
	}
	public void end() {
		if (running) {
			try {
				in.close();
				out.close();
				connection.close();
				running = false;
			} catch (Exception e) {
				System.out.println("Error! Could not end program\n");
				Button.waitForAnyPress();
				return;
			}
		}
	}
}
